#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "hardware.h"
#include "GravitySensor.h"
#include "timer.h"
#include "tick.h"
#include "package.h"

#undef TRACE_LEVEL
#undef TRACE_TITLE
#undef TRACE_TITLE
#define TRACE_LEVEL	TRACE_LEVEL_NO_TRACE
#define TRACE_NAME  TRACE_NAME_DISABLE
#define TRACE_TITLE "MMA8451"
#include "trace.h"


extern uint8_t PinMask2PinSource(uint16_t PinMask);
extern uint8_t Port2PortSource(GPIO_TypeDef *port);

static int Enable = 0;
static int DataReady = 0;
static uint8_t AxisData[6];
uint32_t sEETimeout;


static int I2CRead(uint8_t address, uint8_t *buffer, int len);
static int I2CWrite(uint8_t address, uint8_t *buffer, int len);


#define I2C_SDA_PORT	GRAVITY_I2C_PORT
#define I2C_SCL_PORT	GRAVITY_I2C_PORT
#define I2C_SDA_PIN		GRAVITY_I2C_SDA_PIN
#define I2C_SCL_PIN		GRAVITY_I2C_SCL_PIN

// #define INT1_PORT		GPIOB
// #define INT1_PIN		GPIO_Pin_5
// #define INT1_PORT_SRC	GPIO_PortSourceGPIOB
// #define INT1_PIN_SRC	GPIO_PinSource5
// #define INT1_EXT_LINE	EXTI_Line5

#define I2C_SLAVE_ADDRESS7	0x1C
#define I2C_SPEED			1000000

#define sEE_FLAG_TIMEOUT         ((uint32_t)0x1000)
#define sEE_LONG_TIMEOUT         ((uint32_t)(10 * sEE_FLAG_TIMEOUT))


static void I2COpen(void) {
	GPIO_InitTypeDef  GPIO_InitStructure;
	I2C_InitTypeDef  I2C_InitStructure;
    
	/*!< sEE_I2C_SCL_GPIO_CLK and sEE_I2C_SDA_GPIO_CLK Periph clock enable */
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

	// Enable I2C clock
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);

	/*!< GPIO configuration */  
	/*!< Configure sEE_I2C pins: SCL */
	GPIO_InitStructure.GPIO_Pin = I2C_SCL_PIN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
	GPIO_Init(I2C_SCL_PORT, &GPIO_InitStructure);

	/*!< Configure sEE_I2C pins: SDA */
	GPIO_InitStructure.GPIO_Pin = I2C_SDA_PIN;
	GPIO_Init(I2C_SDA_PORT, &GPIO_InitStructure);

	I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
	I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
	I2C_InitStructure.I2C_OwnAddress1 = I2C_SLAVE_ADDRESS7;
	I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
	I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
	I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;

	/* sEE_I2C Peripheral Enable */
	I2C_Cmd(I2C1, ENABLE);
	
	/* Apply sEE_I2C configuration after enabling it */
	I2C_Init(I2C1, &I2C_InitStructure);  
}

static void I2CClose() {
	GPIO_InitTypeDef  GPIO_InitStructure;
	
	GPIO_InitStructure.GPIO_Pin = I2C_SCL_PIN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(I2C_SCL_PORT, &GPIO_InitStructure);

	/*!< Configure sEE_I2C pins: SDA */
	GPIO_InitStructure.GPIO_Pin = I2C_SDA_PIN;
	GPIO_Init(I2C_SDA_PORT, &GPIO_InitStructure);
	
	I2C_Cmd(I2C1, DISABLE);
	
	I2C_DeInit(I2C1);
}

static int I2CWrite(uint8_t address, uint8_t *buffer, int len) {
	// sEEDataWritePointer = NumByteToWrite;  

	/*!< While the bus is busy */
	sEETimeout = sEE_LONG_TIMEOUT;
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY))
	{
	if((sEETimeout--) == 0) return -1; // sEE_TIMEOUT_UserCallback();
	}

	/*!< Send START condition */
	I2C_GenerateSTART(I2C1, ENABLE);

	/*!< Test on EV5 and clear it */
	sEETimeout = sEE_FLAG_TIMEOUT;
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT))
	{
	if((sEETimeout--) == 0) return -1; // sEE_TIMEOUT_UserCallback();
	}

	/*!< Send EEPROM address for write */
	I2C_Send7bitAddress(I2C1, I2C_SLAVE_ADDRESS7 << 1, I2C_Direction_Transmitter);

	/*!< Test on EV6 and clear it */
	sEETimeout = sEE_FLAG_TIMEOUT;
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
	{
	if((sEETimeout--) == 0) return -1; //sEE_TIMEOUT_UserCallback();
	}

 	/*!< Send the EEPROM's internal address to write to : only one byte Address */
 	I2C_SendData(I2C1, address);

	/*!< Test on EV8 and clear it */
	sEETimeout = sEE_FLAG_TIMEOUT; 
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
	{
	if((sEETimeout--) == 0) return -1; // sEE_TIMEOUT_UserCallback();
	}
	
	while (len --) {
		I2C_SendData(I2C1, *(buffer ++));
		
		if(len == 0) {
			I2C_GenerateSTOP(I2C1, ENABLE);	
		}	
		
		sEETimeout = sEE_FLAG_TIMEOUT; 
		while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
		{
		if((sEETimeout--) == 0) return -1; // sEE_TIMEOUT_UserCallback();
		} 
	}
	return 0;
}

static int I2CRead(uint8_t address, uint8_t *buffer, int len) {
	/*!< While the bus is busy */
	sEETimeout = sEE_LONG_TIMEOUT;
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY)) {
		if((sEETimeout--) == 0) 
			return -1;
	}

	/*!< Send START condition */
	I2C_GenerateSTART(I2C1, ENABLE);

	/*!< Test on EV5 and clear it (cleared by reading SR1 then writing to DR) */
	sEETimeout = sEE_FLAG_TIMEOUT;
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT)) {
		if((sEETimeout--) == 0) 
			return -1; //sEE_TIMEOUT_UserCallback();
	}

	/*!< Send EEPROM address for write */
	I2C_Send7bitAddress(I2C1, I2C_SLAVE_ADDRESS7 << 1, I2C_Direction_Transmitter);

	/*!< Test on EV6 and clear it */
	sEETimeout = sEE_FLAG_TIMEOUT;
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) {
		if((sEETimeout--) == 0)
			return -1;
	}  

	/*!< Send the EEPROM's internal address to read from: Only one byte address */
	I2C_SendData(I2C1, address);  

	sEETimeout = sEE_FLAG_TIMEOUT;
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BTF) == RESET) {
	if((sEETimeout--) == 0) return -1;//sEE_TIMEOUT_UserCallback();
	}

	/*!< Send STRAT condition a second time */  
	I2C_GenerateSTART(I2C1, ENABLE);

	/*!< Test on EV5 and clear it (cleared by reading SR1 then writing to DR) */
	sEETimeout = sEE_FLAG_TIMEOUT;
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT))
	{
	if((sEETimeout--) == 0) return -1;//sEE_TIMEOUT_UserCallback();
	} 

	/*!< Send EEPROM address for read */
	I2C_Send7bitAddress(I2C1, I2C_SLAVE_ADDRESS7 << 1 | 1, I2C_Direction_Receiver); 

	sEETimeout = sEE_FLAG_TIMEOUT;
    while(I2C_GetFlagStatus(I2C1, I2C_FLAG_ADDR) == RESET)
    {
      if((sEETimeout--) == 0) return -1;//sEE_TIMEOUT_UserCallback();
    } 

	/* If number of data to be read is 1, then DMA couldn't be used */
	/* One Byte Master Reception procedure (POLLING) ---------------------------*/
	I2C_AcknowledgeConfig(I2C1, ENABLE);   
	
	while (len --) {
		(void)I2C1->SR2;
		
		if(len == 0) {
			I2C_AcknowledgeConfig(I2C1, DISABLE);
			I2C_GenerateSTOP(I2C1, ENABLE);			
		}
	
		/* Wait for the byte to be received */
		sEETimeout = sEE_FLAG_TIMEOUT;
		while(I2C_GetFlagStatus(I2C1, I2C_FLAG_RXNE) == RESET)
		{
		  if((sEETimeout--) == 0) return -1;//sEE_TIMEOUT_UserCallback();
		}
    
		/*!< Read the byte received from the EEPROM */
		*(buffer ++) = I2C_ReceiveData(I2C1);
	}
	return 0;
}


static int ReadMMA8451Register(uint8_t address) {
	uint8_t buffer;
	
	if(I2CRead(address, &buffer, 1) == -1) {
		return -1;
	}
	return (int)buffer;
}

static int WriteMMA8451Register(uint8_t address, uint8_t value) {
	return I2CWrite(address, &value, 1);
}

static __inline int MMA845xStandby() {
	int ctrl;
	
	ctrl = ReadMMA8451Register(0x2A);
	if(ctrl == -1)
		return -1;
	if(ctrl & 1) {
		if(WriteMMA8451Register(0x2A, ctrl & 0xFE) != 0)
			return -1;
	} 
	return ctrl & 1;
}

static __inline int MMA845xActive() {
	int ctrl;
	
	ctrl = ReadMMA8451Register(0x2A);
	if(ctrl == -1)
		return -1;
	if((ctrl & 1) == 0) {
		if(WriteMMA8451Register(0x2A, ctrl | 1) != 0)
			return -1;
	}
	return ctrl & 1;
}

static void INT1Init() {
	GPIO_InitTypeDef GPIO_InitStructure;
	EXTI_InitTypeDef   EXTI_InitStructure;
	
	// Config IO as input pull-up
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_InitStructure.GPIO_Pin = GRAVITY_INT_PIN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GRAVITY_INT_PORT, &GPIO_InitStructure);
	
	// Config EXT-Interrupt mode in GPIO 
	GPIO_EXTILineConfig(Port2PortSource(GRAVITY_INT_PORT), PinMask2PinSource(GRAVITY_INT_PIN));
	
	// Config EXT-Interrupt mode in EXTI
	EXTI_InitStructure.EXTI_Line = GRAVITY_EXTI_LINE;
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;  
	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
	EXTI_Init(&EXTI_InitStructure);
}

static void INT1DeInit() {
	GPIO_InitTypeDef GPIO_InitStructure;
	EXTI_InitTypeDef   EXTI_InitStructure;
	
	// Config IO as input pull-up
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = GRAVITY_INT_PIN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GRAVITY_INT_PORT, &GPIO_InitStructure);
	
	// Config EXT-Interrupt mode in EXTI
	EXTI_InitStructure.EXTI_Line = EXTI_Line14;
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;  
	EXTI_InitStructure.EXTI_LineCmd = DISABLE;
	EXTI_Init(&EXTI_InitStructure);
}

void MMA8451DataReadyIntHandler() {
	uint8_t IntSource;
	
	IntSource = ReadMMA8451Register(0x0C);
	
	// Data ready interrupt
	if (IntSource & 1) {
		I2CRead(0x01, AxisData, 6);
		DataReady = TRUE;
	}
}

int CloseGravitySensor(void) {
	INT1DeInit();
	I2CClose();
	Enable = FALSE;
	DataReady = FALSE;
	return 0;
}

int OpenGrayvitySensor(void) {
	uint8_t buffer;
	
	if (Enable == TRUE) {
		CloseGravitySensor();
	}
	
	I2COpen();
	
	// Check the device ID
	if(ReadMMA8451Register(0x0D) != 0x1A) {
		I2CClose();
		return -1;
	}
	// MMA845xStandby();
	// reset devie to default state
	WriteMMA8451Register(0x2B, 0x40);
	delay_ms(1);
	WriteMMA8451Register(0x2B, 0x00);
	
	// into standby mode to config basic settings
	MMA845xStandby();
	
	// default gravity range is 2G 
	WriteMMA8451Register(0x0E, 0x00); 
	
	// data output rate set to 100Hz
	buffer = ReadMMA8451Register(0x2A);
	buffer &= ~(7 << 3);
	buffer |= (3 << 3);
	WriteMMA8451Register(0x2A, buffer);
	
	// High Resolution Oversampling mode
	buffer = ReadMMA8451Register(0x2B);
	buffer &= ~(3 << 0);
	buffer |= 2;
	WriteMMA8451Register(0x2B, buffer);
	
	// Open drain on interrupt Pin
	// WriteMMA8451Register(0x2C, 0x01);
	
	// Enable data ready interrupt to INT1
	WriteMMA8451Register(0x2D, 0x01);
	WriteMMA8451Register(0x2E, 0x01);
	
	// Init the INT1 Line interrupt
	INT1Init();
	
	// active the device
	MMA845xActive();
	
	Enable = TRUE;
	return 0;
}

int WriteGravitySensorRegister(uint8_t address, uint8_t value) {
	int ret;
	
	MMA845xStandby();
	ret = WriteMMA8451Register(address, value);
	MMA845xActive();
	return ret;
}

int ReadGravitySensorRegister(uint8_t address) {
	int ret;
	
	MMA845xStandby();
	ret = ReadMMA8451Register(address);
	MMA845xActive();
	return ret;
}

int SelectGravitySensorRange(GravitySensorRange_t range) {
	uint8_t buffer;
	uint8_t status;
	int error;
	
	status = MMA845xStandby();
	
	switch(range) {
	case GRAVITY_RANGE_2G:
		buffer = 0x00;
	break;
	case GRAVITY_RANGE_4G:
		buffer = 0x01;
	break;
	case GRAVITY_RANGE_8G:
		buffer = 0x02;
	break;
	default:
		if(status & 1)
			MMA845xActive();
		return -1;
	}
	error = WriteMMA8451Register(0x0E, buffer);
	
	if(status & 1) 
		MMA845xActive();
	return error;
}

int SetGravitySamplingFreqency(GravitySensorSamplingFrequency_t freq) {
	uint8_t buffer;
	uint8_t status;
	int error = 0;
	
	status = MMA845xStandby();
	
	buffer = ReadMMA8451Register(0x2A);
	buffer &= ~(7 << 3);
	
	switch(freq) {
	case GRAVITY_SAMPLING_800HZ:
		buffer |= (0 << 3);
	break;
	case GRAVITY_SAMPLING_400HZ:
		buffer |= (1 << 3);
	break;
	case GRAVITY_SAMPLING_200HZ:
		buffer |= (2 << 3);
	break;
	case GRAVITY_SAMPLING_100HZ:
		buffer |= (3 << 3);
	break;
	case GRAVITY_SAMPLING_50HZ:
		buffer |= (4 << 3);
	break;
	case GRAVITY_SAMPLING_12HZ5:
		buffer |= (5 << 3);
	break;
	case GRAVITY_SAMPLING_6HZ25:
		buffer |= (6 << 3);
	break;
	case GRAVITY_SAMPLING_1HZ56:
		buffer |= (7 << 3);
	break;
	default:
		if(status & 1) 
			MMA845xActive();
		return -1;
	}
	
	error = WriteMMA8451Register(0x2A, buffer);
	
	if(status & 1) 
		MMA845xActive();
	return error;
}

int isGravitySensorEnable(void) {
	return Enable;
}

void OutputGravitySensorData(void) {
	uint16_t buffer[3];
	
	if (DataReady == TRUE) {
		DataReady = FALSE;
		buffer[0] = AxisData[0] << 8 | AxisData[1];
		buffer[1] = AxisData[2] << 8 | AxisData[3];
		buffer[2] = AxisData[4] << 8 | AxisData[5];

		TRACE_DEBUG("GRAVITY_DATA[0x%x][0x%x][0x%x]", buffer[0], buffer[1], buffer[2]);
		WritePackage(PKG_GRAVITY_DATA, get_tick_count(), buffer, 3);
	}
}


/* TEST ONLY */
int MMA8451Main() {
	OpenGrayvitySensor();
	// TRACE_DEBUG("sizeof(short)=%d", sizeof(short));
	while (1) {
		OutputGravitySensorData();
	}
	return 0;
}
